#include <stdio.h>
#include <iostream>
#include <stdlib.h>
#include <string>
#include <vector>
#include <math.h>
#include <boost/asio.hpp>
#include <boost/asio/serial_port.hpp>
#include <boost/system/error_code.hpp>
#include <boost/system/system_error.hpp>
#include <boost/bind.hpp>
#include <boost/thread.hpp>

using namespace std;
#define head1 0xCD
#define head2 0xAB
#define START_FRAME 0xABCD


enum packetFinderState{
    waitingForHead1,
    waitingForHead2,
    waitingForPayloadSize,
};

typedef struct{
    uint16_t  start;          //0XABCD
    int16_t   rpmR;    	      //右轮转速
    int16_t   rpmL;    	      //左轮转速
    int16_t   batVoltage;     //主板电压
    int16_t   boardTemp;      //主板温度
    int16_t   curL_DC;        //左电机电流
    int16_t   curR_DC;        //右电机电流
    uint16_t  checksum;       //校验和
} RikibotFeedback;

typedef struct{
   uint16_t start;
   int16_t  mSpeedR;
   int16_t  mSpeedL;
   uint16_t checksum;
} RikibotCommand;



typedef boost::shared_ptr<boost::asio::serial_port> serialp_ptr;

boost::system::error_code ec_;
boost::asio::io_service io_service_;
serialp_ptr sp_;

//string port_name = "/dev/ttyUSB0";
string port_name = "/dev/ttyTHS1";
int baud_rate = 115200;
RikibotFeedback Feedback;
RikibotCommand  Command;
packetFinderState state_;


bool init_serial()
{
    if(sp_){
        cout << "The SerialPort is already opened!" << endl;
        return false;
    }
    sp_ = serialp_ptr(new boost::asio::serial_port(io_service_));
    sp_->open(port_name, ec_);
    if(ec_){
        cout <<"error : port_->open() failed...port_name=" << port_name << ", e=" << ec_.message().c_str();
        return false;
    }
    sp_->set_option(boost::asio::serial_port_base::baud_rate(baud_rate));
    sp_->set_option(boost::asio::serial_port_base::character_size(8));
    sp_->set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one));
    sp_->set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none));
    sp_->set_option(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none));

    return true;
}

void handle_base_data()
{
    printf("system rpmr: %d, rpml %d\r\n", Feedback.rpmR, Feedback.rpmL);
    printf("system volt: %f, temp:%f\r\n", (float)Feedback.batVoltage/100,(float)Feedback.boardTemp/10);
}

void distribute_data(uint8_t* buffer_data)
{
     uint16_t checksum = 0;
     memcpy(&Feedback, buffer_data, sizeof(Feedback));
     handle_base_data();
}

void rx_foc_data()
{
    uint8_t buffer_data[255];
    //boost::asio::read(*sp_.get(),boost::asio::buffer(&buffer_data, sizeof(Feedback)),ec_);

    switch (state_){
        case waitingForHead1:
            boost::asio::read(*sp_.get(), boost::asio::buffer(&buffer_data[0], 1), ec_);
            state_ = buffer_data[0] == head1 ? waitingForHead2 : waitingForHead1;
            if(state_ == waitingForHead1){
                printf("recv head1 error : -> %x\r\n", buffer_data[0]);
            }
            break;
        case waitingForHead2:
            boost::asio::read(*sp_.get(),boost::asio::buffer(&buffer_data[1],1),ec_);
            state_ = buffer_data[1] == head2 ? waitingForPayloadSize : waitingForHead1;
            if(state_ == waitingForHead1) {
                printf("recv head2 error : -> %x\r\n", buffer_data[1]);
            }
            break;
        case waitingForPayloadSize:
            boost::asio::read(*sp_.get(),boost::asio::buffer(&buffer_data[2],sizeof(Feedback)-2),ec_);
            distribute_data(buffer_data);
            state_ = waitingForHead1;
            break;
        default:
            state_ = waitingForHead1;
            break;
        }
}

void send_speed(int left_speed, int right_speed)
{
    Command.start = (uint16_t)START_FRAME;
    Command.mSpeedR = (uint16_t)right_speed;
    Command.mSpeedL = (uint16_t)left_speed;
    Command.checksum = (uint16_t)(Command.start^Command.mSpeedR^Command.mSpeedL);
    boost::asio::write(*sp_.get(),boost::asio::buffer(&Command, sizeof(Command)),ec_);
}

int main(int argc, char *argv[])
{
    bool serial_status = init_serial();
    state_ = waitingForHead1;

    while(serial_status){
        rx_foc_data();
        send_speed(100, -100);
        usleep(20000);
    }
    return 0;
}
